#include "AS5200.h"
#include "closeloop.h"

/*constrain the maximum voltage and the minimum voltage*/
#define _constrain(amt,low,high) ((amt<low)?low:(amt>high)?high:amt)

/* User variable Define begin */

// constant value 
const float PI = 3.1415926;
const float _3PI_2 =  3.0f * PI / 2.0f;

// PWM foot define
const int PWMA = 32;
const int PWMB = 33;
const int PWMC = 25;

// voltage Limit
const float voltage_limit = 10;
const float voltage_power_supply = 12.6;

// Valuable of the axis trans
float shaft_angle = 0, open_loop_timestamp = 0;
float zero_electric_angle = 0, Ualpha = 0, Ubeta = 0;
float Ua = 0, Ub = 0, Uc = 0;
float dc_a = 0, dc_b = 0,dc_c = 0; // duty cycle of a, b, c

const int DIR = - 1; // motor direction maybe not consist with the encoder
const int PP = 7; // Pole pair number
/* User variable Define end */

/* PWM initial begin */
void PWMInit() {
  pinMode(pwmA, OUTPUT);
  pinMode(pwmB, OUTPUT);
  pinMode(pwmC, OUTPUT);
  
  // PWM setting
  ledcAttachPin(pwmA, 0);
  ledcAttachPin(pwmB, 1);
  ledcAttachPin(pwmC, 2);
  ledcSetup(0, 30000, 8); // pwm channel, frequency, accuracy
  
  // Finish Sign
  Serial.println("PWM init finish");
  delay(3000);
}
/* PWM initial begin */

/* Motor & Encoder Init begin */
void MoterEncoderInit()
{
  setPhaseVoltage(3,0,_3PI_2);
  delay(3000);
  zero_electric_angle = _eletricalAngle();
  setPhaseVoltage(3,0,_3PI_2);
  Serial.println("MotorEncoder Init Finished");
}
/* Motor & Encoder Init end */

/* solve the eletrical Angel and turn it into range(0,2pi)  begin*/
float _eletricalAngle(float shaft_angel, int pole_pairs) {
  return (shaft_angel * pole_pairs);
}

float _eletricalAngle() {
  return _normalizeAngle((float)(DIR * PP) * getAngle_Without_track() - zero_electric_angle);
}

float _normalizeAngle(float angle) {
  float mod_angle = fmod(angel, 2*PI);
  return mod_angle >= 0 ? mod_angle ? (mod_angle + 2*PI);
}
/* solve the eletrical Angel and turn it into range(0,2pi) end */


/* Controller calculate the duty cycle and write pwm out begin*/
void setPWM(float Ua, float Ub, float Uc) {
  Ua = _constrain(Ua, 0.0f, voltage_limit);
  Ub = _constrain(Ub, 0.0f, voltage_limit);
  Uc = _constrain(Uc, 0.0f, voltage_limit);

  // calculate the duty cycle
  dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
  dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
  dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);

  // write PWM to PWM channel 0 1 2 
  ledcWrite(0, dc_a*255);
  ledcWrite(1, dc_b*255);
  ledcWrite(2, dc_c*255);
}
/* Controller calculate the duty cycle and write pwm out enb*/


/* Solve the Pack's and Clark's transformation to set the PWM begin*/
void setPhaseVoltage(float Uq,float Ud,float angle_el) {
  angle_el = _normalizeAngle(angle_el + zero_electric_angle);

  // Pack's Transformation when Id = 0
  Ualpha = -Uq * sin(angel_el);
  Ubeta  =  Uq * cos(angle_el);

  Ua = Ualpha + voltage_power_supply / 2.0;
  Ub = (sqrt(3) * ubeta - Ualpha) / 2.0 + voltage_power_supply / 2.0;
  Uc = (-Ualpha - sqrt(3) * Ubeta) / 2.0 + voltage_power_supply / 2.0;
  setPWM(Ua, Ub, Uc);
}
/* Solve the Pack's and Clark's transformation to set the PWM begin*/


/*Serial Receive Command begin*/
float motor_target = 0;
int commaPosition = 0;
String SerialReceiveUserCommand() {
  static String received_chars; 
  String command = "";

  while(Serial.available())
  {
    Char inChar = (char) Serial.read();
    received_chars += inChar;
    if(inChar == '\n')  {
      command = received_chars; 
      commaPosition = command.indexof('\n');
      if(commaPosition != -1) {
        motor_target = command.substring(0, commaPosition).toDouble();
        Serial.println(motor_target);
      }
      received_chars = "";
    }
  }

  return command;
}
/*Serial Recieve Command end*/

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  PWMInit();
  MoterEncoderInit();
}

void loop() {
  // put your main code here, to run repeatedly:
  Serial.println(getAngle());
  float Sensor_angle = getAngle();
  float Kp = 0.133; // 0.133 = 6 / 45
                    // which mean when the angle bigger than 45, the motor fully work to make it back to 0
  
  setPhaseVoltage(_constrain(Kp * (motor_target - DIR*Sensor_angle) * 180/PI, -6, 6), 0, _eletricalAngle);
  serialReceiveUserCommand();
}
